#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Hubs,  S2, HTMotor,  HTServo,  none,     none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Sensor, S2,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorFR,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C1_2,     motorFL,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_1,     motorBR,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     motorBL,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_1,     motorFan1,     tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S2_C1_2,     motorFan2,     tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    servoFR,              tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    servoFL,              tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    servoBR,              tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    servoBL,              tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    servoGrabber,         tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo1,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_1,    servoTube1,           tServoStandard)
#pragma config(Servo,  srvo_S2_C2_2,    servoTube2,           tServoStandard)
#pragma config(Servo,  srvo_S2_C2_3,    servo2,               tServoNone)
#pragma config(Servo,  srvo_S2_C2_4,    servo3,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_5,    servo4,              tServoNone)
#pragma config(Servo,  srvo_S2_C2_6,    servo5,              tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "../ftc6220/Includes/manipulators.c"

#include "JoystickDriver.c"



void EnableGoalGrabber(bool state)
{
	if (state == true)
	{
		servo[servoGrabber] = 255;
	}
	else
	{
		servo[servoGrabber] = 130;
	}
}

void DriveRightMotors(int pow)
{
	motor[motorFL] = pow;
	motor[motorBL] = pow;
}

void DriveLeftMotors(int pow)
{
	motor[motorBR] = pow;
	motor[motorFR] = pow;
}


task main()
{
	bool grabberReady = true;
	bool grabbed = false;
	int slow = 1;
	while(true)
	{
		getJoystickSettings(joystick);
		//   !!!   begin goal grabber   !!!   //
		if ((joystick.joy1_Buttons == 1) & grabberReady == true)
		{
			grabberReady = false;
			if (grabbed == false)
			{
				grabbed = true;
				EnableGoalGrabber(true);
			}
			else
			{
				grabbed = false;
				EnableGoalGrabber(false);
			}
		}
		else if (joystick.joy1_Buttons != 1)
		{
			grabberReady = true;
		}
		///   !!!   end goal grabber   !!!   ///

		if (abs(joystick.joy1_y1) > 20)
		{
			DriveLeftMotors(joystick.joy1_y1 * slow);
		}
		else
		{
			DriveLeftMotors(0);
		}
		if (abs(joystick.joy1_y2) > 20)
		{
			DriveRightMotors(joystick.joy1_y2 * slow);
		}
		else
		{
			DriveRightMotors(0);
		}

		if (joystick.joy1_Buttons == 6)
		{
			slow = 0.4;
		}
		else
		{
			slow = 1;
		}
	}
}
